# -- coding:utf-8 --
#!/usr/bin/python
import sys, os
sys.path.append("/home/avatar/Minibot/04.RobotSamples/02.CarRobot")
import inspect
from BwSensorLib import BwSensorLib
from CarRobot import CarRobot
from BwRobotLib import BwRobotLib
from MiniBotControlLib import MiniBotControlLib
import time
import BwHelpLib 

msg = """
请选择轮子模式 :
2 : 两轮
3 : 三轮
4 : 四向轮
5 : 四麦克朗姆轮
6 : 四并橡胶轮
CTRL-C to quit
"""

class PatrolCar:
    def __init__(self):
        # 实例化MiniBotControlLib类，并设置为udp模式
        self.mlib = MiniBotControlLib("udp")
        # 输入ip地址和端口号
        self.ip = "10.10.100.254"
        self.port = 8899
        # 连接机器人,并设置超时时间2s
        self.mlib.Connect(self.ip, self.port, 2000)
        # 实例化BwRobotLib类，并将mlib作为参数传入
        self.robotlib = BwRobotLib(self.mlib)
        # 实例化carRobot类，并将mlib作为参数传入
        self.carRobot = CarRobot(self.mlib)
        # 实例化传感器库对象
        self.sensorlib = BwSensorLib(self.mlib)
        # 根据模块连接位置，自动加载ID配置,print_flag设为True，可打印打印当前模块信息
        self.robotlib.module_ids = self.robotlib.scanNConfigCarIds(print_flag=True)
        # 配置轮式模式 2:两轮   3:三轮   4:四向轮  5:四麦克朗姆轮  6:四并橡胶轮
        print(msg)
        self.wheel_mode = BwHelpLib.inputNum()
        # 流水灯模式 1:右转  2：左转 3：报警闪光  5：流水灯（初始化）
        self.led_mode = 5
        # 配置机器人（包括ID位置与速度模式）
        # 配置ID:ids F(4),B(11),l1(8),l2(8),l3(10),l4(11),r1(4),r2(7),r3(6),r4(6) 【TOP:9】
        self.carRobot.bindIds(self.robotlib.module_ids, self.wheel_mode)
        # 延时时间
        self.wait_time_long = 3
        self.wait_time_short = 0.1
        # 设置小车速度
        self.speed = 50
        self.carRobot.update_speed(self.speed)

    # 巡线小车
    def patrol(self):
        tracking = self.sensorlib.readTrackingSensor()  # 背面看传感器 得出数组元素分别为右传感器(ir1)、左传感器（ir2）
        time.sleep(0.1) 
        print(tracking)
        # ir1检测到黑色，ir2检测到黑色
        if (tracking[0] == 0 and tracking[1] == 0):  
            self.carRobot.goForward()
            print("红外传感器未超限，巡线机器人向前进")
            time.sleep(self.wait_time_short)
        # ir1检测到黑色，ir2检测到非黑色
        elif (tracking[0] == 0 and tracking[1] == 1):
            self.carRobot.turnRight()
            print("左红外传感器超限，巡线机器人向右转")
            time.sleep(self.wait_time_short)
        # ir1检测到非黑色，ir2检测到黑色
        elif (tracking[0] == 1 and tracking[1] == 0):
            self.carRobot.turnLeft()
            print("右红外传感器超限，巡线机器人向左转")
            time.sleep(self.wait_time_short)
        # ir1检测到非黑色，ir2检测到非黑色
        elif (tracking[0] == 1 and tracking[1] == 1):
            self.carRobot.goBack()         
            print("左右红外传感器均超限，巡线机器人向后退")
            time.sleep(self.wait_time_long)
       
if __name__ == "__main__":
    # 实例化巡线小车类
    patrol_car = PatrolCar()    
    try:
        while 1 :
            # 执行巡线小车功能
            patrol_car.patrol()
    except KeyboardInterrupt:
        patrol_car.carRobot.stop() # 小车停止运动
        print(" Program closed! ")
    pass

 
